// beep.c
//

#include <avr/io.h>
#include <stdio.h>

#include "../mcu/config.h"
#include "../mcu/types.h"

#include "common.h"


#define BEEP_DISCR_THRESHOLD   30

#define VARIO_MAX_CPS   	1000L
#define VARIO_XOVER_CPS		400L

#define VARIO_MAX_FREQHZ    4800L
#define VARIO_XOVER_FREQHZ  2000L
#define VARIO_MIN_FREQHZ    400L

#define VARIO_SINK_CPS   	-300L
#define VARIO_SINK_FREQHZ   800L

#define CLIMBTONE_CPS_MIN   -30
#define CLIMBTONE_CPS_DEF   0
#define CLIMBTONE_CPS_MAX   +30

#define SINKTONE_CPS_MIN   -100
#define SINKTONE_CPS_DEF   -300
#define SINKTONE_CPS_MAX   -400

#define ABS(x)				((x) >= 0 ? (x) : (-(x)))


short gnBeepPeriodTicks = 0;
short gnBeepTransition;
long gnVarioCps = 0;

short gnSinkToneCps = SINKTONE_CPS_MIN;
short gnClimbToneCps = CLIMBTONE_CPS_MAX;


const u8 gnBeepTbl[20][2] = 
{
#if 0
	{35,28}, // 0.0 - 0.5
	{30,24}, // 0.5 - 1.0
	{25,20}, // 1.0 - 1.5
	{20,16}, // 1.5 - 2.0
	{18,14}, // 2.0 - 2.5
	{16,11}, // 2.5 - 3.0
	{14,10}, // 3.0 - 3.5
	{12,8}, // 3.5 - 4.0
	{11,8}, // 4.0 - 4.5
	{10,7}, // 4.5 - 5.0
	{9,6}, // 5.0 - 5.5
	{8,6}, // 5.5 - 6.0
	{7,5}, // 6.0 - 6.5
	{7,5}, // 6.5 - 7.0
	{7,5}, // 7.0 - 7.5
	{7,5}, // 7.5 - 8.0
	{7,5}, // 8.0 - 8.5
	{7,5}, // 8.5 - 9.0
	{7,5}, // 9.0 - 9.5
	{7,5} // 9.5 - 10.0
#else
	{25,11}, // 0.0 - 0.5
	{22,11}, // 0.5 - 1.0
	{18,9}, // 1.0 - 1.5
	{14,7}, // 1.5 - 2.0
	{13,6}, // 2.0 - 2.5
	{11,6}, // 2.5 - 3.0
	{10,5}, // 3.0 - 3.5
	{9,5}, // 3.5 - 4.0
	{8,4}, // 4.0 - 4.5
	{7,3}, // 4.5 - 5.0
	{6,3}, // 5.0 - 5.5
	{5,2}, // 5.5 - 6.0
	{4,2}, // 6.0 - 6.5
	{4,2}, // 6.5 - 7.0
	{4,2}, // 7.0 - 7.5
	{4,2}, // 7.5 - 8.0
	{4,2}, // 8.0 - 8.5
	{4,2}, // 8.5 - 9.0
	{4,2}, // 9.0 - 9.5
	{4,2} // 9.5 - 10.0
#endif
};


#define BEEP_DDR	DDRD
#define BEEP_PORT	PORTD
#define BEEP_BIT	PD7

void beep_Init()
{
	// PB7 -> OC2 -> set as output
	BEEP_DDR |= (1 << BEEP_BIT);
	BEEP_PORT &= ~(1 << BEEP_BIT);

	TCNT2 = 0;
	OCR2 = 0;
	// WMG2[1:0] -> 10 -> CTC Mode
	// COM2[1:0] -> 00 -> Normal port operation, OC2 disconnected 
	// CS2[2:0] -> 110 -> 256 prescale
	//             101 -> 128 prescale
	TCCR2 = (1 << WGM21) | (0 << WGM20) | (0 << COM21) | (0 << COM20) | (1 << CS22) | (0 << CS21) | (1 << CS20);
}

void beep_SetFreq(long nFreqHz)
{
	// Focn = Fclk_io / 2 x N x (1 + OCR2) : N -> prescale factor(1, 8, 64, 256, 1024)
	// OCR2 = Fclk_io / Focn x 2 x N - 1

//	printf("beep freq : %ld\n", nFreqHz);

	if( nFreqHz )
	{
//		TCNT2 = 0;
		OCR2 = (unsigned char)((long)F_CPU / (nFreqHz * 2 * 128)) - 1;
		TCCR2 |= (0 << COM21) | (1 << COM20); // Toggle OC2 in CTC Mode
	}
	else
	{
		TCNT2 = 0;
		OCR2 = 0;
		TCCR2 &= ~((1 << COM21) | (1 << COM20)); // turn to normal port operation
	}
}

void beep_SetCps(long nCps)	
{
	int nRange;
	long nFreqHz, nBeepCps;

//	if ((gnBeepPeriodTicks <= 0) || (ABS(nCps-gnVarioCps) > BEEP_DISCR_THRESHOLD) || ((nCps > 0) && (gnVarioCps <= 0)) ) 
	if( gnBeepPeriodTicks <= 0 )
	{
		if( nCps < gnSinkToneCps ) // if sinking much faster than glider sink rate, generate continuous tone alarm
		{
			gnVarioCps = nCps;
			gnBeepPeriodTicks = 0;
			gnBeepTransition  = 0;

			nFreqHz = VARIO_SINK_FREQHZ + ((gnSinkToneCps - nCps) / 100) * 100;
			beep_SetFreq(VARIO_SINK_FREQHZ);
		}
		else // if climbing, generate beep
		{
			if( nCps > gnClimbToneCps )
			{
				if( ABS(nCps-gnVarioCps) > BEEP_DISCR_THRESHOLD )
					gnVarioCps = nCps;
			}
			else // if  within normal glider sink rate, be quiet
			{
				gnVarioCps = 0;
				gnBeepPeriodTicks = 0;
				gnBeepTransition  = 0;

				beep_SetFreq(0);
			}

			if( gnBeepPeriodTicks <= 0 && gnVarioCps > gnClimbToneCps )
			{
				nBeepCps = gnVarioCps - gnClimbToneCps;

				if (nBeepCps > VARIO_MAX_CPS) 
					nBeepCps = VARIO_MAX_CPS;
					
				nRange = nBeepCps / 50;

				if (nRange > 19) 
					nRange = 19;
					
				gnBeepPeriodTicks = (short)(gnBeepTbl[nRange][0]);
				gnBeepTransition = (short)(gnBeepTbl[nRange][1]); // (short)(gnBeepTbl[nRange][0] - gnBeepTbl[nRange][1]);
				
				if (nBeepCps > VARIO_XOVER_CPS)
				{
					nFreqHz = VARIO_XOVER_FREQHZ + ((nBeepCps - VARIO_XOVER_CPS)*(VARIO_MAX_FREQHZ - VARIO_XOVER_FREQHZ))/(VARIO_MAX_CPS - VARIO_XOVER_CPS);
				}
				else
				{
					nFreqHz = VARIO_MIN_FREQHZ + (nBeepCps * (VARIO_XOVER_FREQHZ - VARIO_MIN_FREQHZ))/VARIO_XOVER_CPS;
				}
				
				beep_SetFreq(nFreqHz);
			}
		}
	}
	else // beep done
	{ 
		gnBeepPeriodTicks--;

		if( gnBeepPeriodTicks <= gnBeepTransition )
		{
			nFreqHz = 0;
			beep_SetFreq(nFreqHz);
		}
	}       	
}
